#include "Geometry/Frustum.h"
#include "Framework/StableHeaders.h"

#include "Camera/Camera.h"
#include "Geometry/Plane.h"


namespace tyro
{
	namespace Geometry
	{
		

		tyro::FLOAT_32 Frustum::GetLeftExtern() const
		{
			return left;
		}

		void Frustum::SetLeftExtern( tyro::FLOAT_32 val )
		{
			left = val;
		}

		tyro::FLOAT_32 Frustum::GetRightExtern() const
		{
			return right;
		}

		void Frustum::SetRightExtern( tyro::FLOAT_32 val )
		{
			right = val;
		}

		tyro::FLOAT_32 Frustum::GetBottomExtern() const
		{
			return bottom;
		}

		void Frustum::SetBottomExtern( tyro::FLOAT_32 val )
		{
			bottom = val;
		}

		tyro::FLOAT_32 Frustum::GetTopExtern() const
		{
			return top;
		}

		void Frustum::SetTopExtern( tyro::FLOAT_32 val )
		{
			top = val;
		}

		tyro::FLOAT_32 Frustum::GetNearClipDistance() const
		{
			return nearVal;
		}

		void Frustum::SetNearClipDistance( tyro::FLOAT_32 val )
		{
			nearVal = val;
		}

		tyro::FLOAT_32 Frustum::GetFarClipDistance() const
		{
			return farVal;
		}

		void Frustum::SetFarClipDistance( tyro::FLOAT_32 val )
		{
			farVal = val;
		}

		tyro::FLOAT_32 Frustum::GetAspectRatio() const
		{
			return aspectRatio;
		}

		void Frustum::SetAspectRatio( tyro::FLOAT_32 val )
		{
			aspectRatio = val;
		}

		Math::Matrix4f Frustum::GetProjectionMatrix() const
		{
			return projectionMatrix;
		}

		void Frustum::SetProjectionMatrix( Math::Matrix4f val )
		{
			projectionMatrix = val;
		}

		Math::Matrix4f Frustum::GetViewMatrix() const
		{
			UpdateView();
			return viewMatrix;
		}

		void Frustum::SetViewMatrix( Math::Matrix4f val )
		{
			viewMatrix = val;
		}


		const Math::Matrix4f& Frustum::GetProjectionMatrixRS( void ) const
		{
			UpdateFrustum();

			return projectionMatrixRS;
		}

		void Frustum::UpdateFrustum(void) const
		{
			//if(!updated)
			{
				UpdateFrustumImpl();
			}
		}

		void Frustum::UpdateFrustumImpl() const
		{
			FLOAT_32 left, right, bottom, top;

			CalcProjection(left, right, bottom, top);

			FLOAT_32 inv_w = 1 / (right - left);
			FLOAT_32 inv_h = 1 / (top - bottom);
			FLOAT_32 inv_d = 1 / (farVal - nearVal);

			if (projectionType == PT_PERSPECTIVE)
			{
				FLOAT_32 A = 2 * nearVal * inv_w;
				FLOAT_32 B = 2 * nearVal * inv_h;
				FLOAT_32 C = (right + left) * inv_w;
				FLOAT_32 D = (top + bottom) * inv_h;
				FLOAT_32 q, qn;

				q = - (farVal + nearVal) * inv_d;
				qn = -2 * (farVal * nearVal) * inv_d;

				projectionMatrix = Math::Matrix4f::ZERO;
				projectionMatrix.m00 = A;
				projectionMatrix.m02 = C;
				projectionMatrix.m11 = B;
				projectionMatrix.m12 = D;
				projectionMatrix.m22 = q;
				projectionMatrix.m23 = qn;
				projectionMatrix.m32 = -1;


			}

			projectionMatrixRS = projectionMatrix;
		}

		void Frustum::CalcProjection( FLOAT_32& left, FLOAT_32& right, FLOAT_32& bottom, FLOAT_32& top ) const
		{
			if( projectionType == PT_PERSPECTIVE)
			{	
				FLOAT_32 thetaY(FOVy * 0.5f);
				FLOAT_32 tanThetaY = tan(thetaY);
				FLOAT_32 tanThetaX = tanThetaY * aspectRatio; 

				FLOAT_32 nearFocal = nearVal / focalLength;
				FLOAT_32 nearOffsetX = frustumOffset.x * nearFocal;
				FLOAT_32 nearOffsetY = frustumOffset.y * nearFocal;

				FLOAT_32 halfWidth = tanThetaX * nearVal;
				FLOAT_32 halfHeight = tanThetaY * nearVal;

				left = - halfWidth + nearOffsetX;
				right = + halfWidth + nearOffsetX;
				bottom = - halfHeight + nearOffsetY;
				top = halfHeight + nearOffsetY;

				this->left = left;
				this->right = right;
				this->top = top;
				this->bottom = bottom;

			}
		}

		void Frustum::UpdateView() const
		{
			//if(!updated)
			{
				UpdateViewImpl();
			}
		}

		void Frustum::UpdateViewImpl() const
		{
			viewMatrix = Math::CreateViewMatrix(parentCamera->GetTranslation(), parentCamera->GetRotation());
		}


		void Frustum::UpdateFrustumPlanesImpl( void ) const
		{
			Math::Matrix4f combo = projectionMatrix * viewMatrix;

			frustumPlanes[FRUSTUM_LEFT_PLANE].normal.x = combo.m30 + combo.m00;
			frustumPlanes[FRUSTUM_LEFT_PLANE].normal.y = combo.m31 + combo.m01;
			frustumPlanes[FRUSTUM_LEFT_PLANE].normal.z = combo.m32 + combo.m02;
			frustumPlanes[FRUSTUM_LEFT_PLANE].d = combo.m33 + combo.m03;

			frustumPlanes[FRUSTUM_RIGHT_PLANE].normal.x = combo.m30 - combo.m00;
			frustumPlanes[FRUSTUM_RIGHT_PLANE].normal.y = combo.m31 - combo.m01;
			frustumPlanes[FRUSTUM_RIGHT_PLANE].normal.z = combo.m32 - combo.m02;
			frustumPlanes[FRUSTUM_RIGHT_PLANE].d = combo.m33 - combo.m03;

			frustumPlanes[FRUSTUM_TOP_PLANE].normal.x = combo.m30 - combo.m10;
			frustumPlanes[FRUSTUM_TOP_PLANE].normal.y = combo.m31 - combo.m11;
			frustumPlanes[FRUSTUM_TOP_PLANE].normal.z = combo.m32 - combo.m12;
			frustumPlanes[FRUSTUM_TOP_PLANE].d = combo.m33 - combo.m13;

			frustumPlanes[FRUSTUM_BOTTOM_PLANE].normal.x = combo.m30 + combo.m10;
			frustumPlanes[FRUSTUM_BOTTOM_PLANE].normal.y = combo.m31 + combo.m11;
			frustumPlanes[FRUSTUM_BOTTOM_PLANE].normal.z = combo.m32 + combo.m12;
			frustumPlanes[FRUSTUM_BOTTOM_PLANE].d = combo.m33 + combo.m13;

			frustumPlanes[FRUSTUM_NEAR_PLANE].normal.x = combo.m30 + combo.m20;
			frustumPlanes[FRUSTUM_NEAR_PLANE].normal.y = combo.m31 + combo.m21;
			frustumPlanes[FRUSTUM_NEAR_PLANE].normal.z = combo.m32 + combo.m22;
			frustumPlanes[FRUSTUM_NEAR_PLANE].d = combo.m33 + combo.m23;

			frustumPlanes[FRUSTUM_FAR_PLANE].normal.x = combo.m30 - combo.m20;
			frustumPlanes[FRUSTUM_FAR_PLANE].normal.y = combo.m31 - combo.m21;
			frustumPlanes[FRUSTUM_FAR_PLANE].normal.z = combo.m32 - combo.m22;
			frustumPlanes[FRUSTUM_FAR_PLANE].d = combo.m33 - combo.m23;




		}

		void Frustum::SetProjectionType( ProjectionType pt )
		{
			projectionType = pt;
		}

		void Frustum::SetFOVy( tyro::FLOAT_32 val )
		{
			FOVy = val;
		}

		void Frustum::SetFocalLength( tyro::FLOAT_32 val )
		{
			focalLength = val;
		}

		void Frustum::Initialise(Camera::Camera* camera)
		{
			SetParentCamera(camera);
			SetFOVy(Math::PI/4.0f);
			SetAspectRatio(1.3333334f);
			SetFocalLength(1.0f);
			SetFrustumOffset(Math::Vector2f::ZERO);
			SetNearClipDistance(5.0f);
			SetFarClipDistance(100000.0f);
			SetProjectionType(PT_PERSPECTIVE);
			UpdateFrustum();
		}

		void Frustum::SetFrustumOffset( const Math::Vector2f& offset )
		{
			frustumOffset = offset;
		}

		const Math::Quaternionf& Frustum::GetOrientation( void ) const
		{
			return Math::Quaternionf::ZERO;
		}

		const Math::Vector3f& Frustum::GetTranslation( void ) const
		{
			return Math::Vector3f::ZERO;
		}

		void Frustum::UpdateFrustumPlanes() const
		{
			UpdateView();
			UpdateFrustum();
			UpdateFrustumPlanesImpl();
		}

		void Frustum::SetParentCamera( Camera::Camera* camera ) 
		{
			parentCamera = camera;
		}



	}
}
